#include <iostream>
#include "HRLocomotion/hr_locomotion.h"

using std::cout;
using std::endl;
int main(){


    std::cout << "Starting test." << std::endl;

    /*
    hrLocomotion::RobotState robot;
    robot.init();
    cout << "Robot " << endl << robot << endl;
    hrLocomotion::VectorXr x(3);
    hrLocomotion::VectorXr y(3);
    hrLocomotion::VectorXr t(3);
    robot.init(x,y,t);
    cout << "Robot " << endl << robot << endl;
    x << 1 ,2 ,3;
    y << 9 , 8 ,7;
    t << 4 , 5 ,6;
    robot.init(x,y);
    cout << "Robot" << endl << robot << endl;
    robot.init(x,y,t);
    cout << "Robot" << endl << robot << endl;
    hrLocomotion::RobotState a = robot;
    cout << "Robot" << endl << a << endl;
    std::cout << "End of test" << std::endl ;
*/
    std::cout << "Starting Bezier test." << std::endl;

    hrLocomotion::VectorXr p0(3);
    p0 << 0 ,0 ,0;
    hrLocomotion::VectorXr p1(3);
    p1 << 0 ,1 ,0;
    hrLocomotion::VectorXr p2(3);
    p2 << 0 ,1 ,1;
    hrLocomotion::VectorXr p3(3);
    p3 << 1 ,1 ,1;
    cout << "Calling static methods" << endl;
    cout << hrLocomotion::BezierCurve::Eval(0.0,p0,p1,p2,p3) << endl;
    cout << hrLocomotion::BezierCurve::Eval(0.5,p0,p1,p2,p3) << endl;
    cout << hrLocomotion::BezierCurve::Eval(1.0,p0,p1,p2,p3) << endl;

    cout << "Calling init methods" << endl;
    hrLocomotion::BezierCurve bezier;
    bezier.init(p0,p1,p2,p3,10);

    cout << bezier.eval(0) << endl << endl;
    cout << bezier.eval(4) << endl << endl;
    cout << bezier.eval(5) << endl << endl;
    cout << bezier.eval(9) << endl << endl;

    cout << bezier.eval(0.0) << endl << endl;
    cout << bezier.eval(0.444444) << endl << endl;
    cout << bezier.eval(0.555555) << endl << endl;
    cout << bezier.eval(1.0) << endl << endl;



    cout << "Calling non init method" << endl;

}
